package star.crust.cartspace.framework;

import java.util.ArrayList;

import javax.vecmath.Matrix3f;

import star.core.util.geometry.FloatXYZ;
import star.core.util.geometry.HTransform;


public class CrtObbMath {
	public static boolean RAPID_OK = true;

	/**
	 * Metodi matematici di supporto per la creazione di
	 * gerarchie OBB e la verifica di collsione
	 */
	public static float[] VcrossV(float[] V1, float[] V2) {
		float[] Vr = new float[3];
		Vr[0] = V1[1]*V2[2] - V1[2]*V2[1];
		Vr[1] = V1[2]*V2[0] - V1[0]*V2[2];
		Vr[2] = V1[0]*V2[1] - V1[1]*V2[0];
		return Vr;
	}

	public static float max(float a, float b, float c) 	{
		float t = a;
		if (b > t) t = b;
		if (c > t) t = c;
		return t;
	}

	public static float min(float a, float b, float c) {
		float t = a;
		if (b < t) t = b;
		if (c < t) t = c;
		return t;
	}

	public static float VdotV(float[] V1, float[] V2) {
		return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2]);
	}

	public static float[] sMxVpV(float s1, float[][]  M1, float[] V1, float[] V2)
	{
		float[] Vr = new float[3];
		Vr[0] = s1 * (M1[0][0] * V1[0] +
				M1[0][1] * V1[1] + 
				M1[0][2] * V1[2]) +
				V2[0];
		Vr[1] = s1 * (M1[1][0] * V1[0] +
				M1[1][1] * V1[1] + 
				M1[1][2] * V1[2]) + 
				V2[1];
		Vr[2] = s1 * (M1[2][0] * V1[0] +
				M1[2][1] * V1[1] + 
				M1[2][2] * V1[2]) + 
				V2[2];
		return Vr;
	}

	public static float[][] MTxM(float[][] M1, float[][] M2)
	{
		float[][] Mr = new float[3][3];
		Mr[0][0] = (M1[0][0] * M2[0][0] +
				M1[1][0] * M2[1][0] +
				M1[2][0] * M2[2][0]);
		Mr[1][0] = (M1[0][1] * M2[0][0] +
				M1[1][1] * M2[1][0] +
				M1[2][1] * M2[2][0]);
		Mr[2][0] = (M1[0][2] * M2[0][0] +
				M1[1][2] * M2[1][0] +
				M1[2][2] * M2[2][0]);
		Mr[0][1] = (M1[0][0] * M2[0][1] +
				M1[1][0] * M2[1][1] +
				M1[2][0] * M2[2][1]);
		Mr[1][1] = (M1[0][1] * M2[0][1] +
				M1[1][1] * M2[1][1] +
				M1[2][1] * M2[2][1]);
		Mr[2][1] = (M1[0][2] * M2[0][1] +
				M1[1][2] * M2[1][1] +
				M1[2][2] * M2[2][1]);
		Mr[0][2] = (M1[0][0] * M2[0][2] +
				M1[1][0] * M2[1][2] +
				M1[2][0] * M2[2][2]);
		Mr[1][2] = (M1[0][1] * M2[0][2] +
				M1[1][1] * M2[1][2] +
				M1[2][1] * M2[2][2]);
		Mr[2][2] = (M1[0][2] * M2[0][2] +
				M1[1][2] * M2[1][2] +
				M1[2][2] * M2[2][2]);
		return Mr;
	}

	public static float[] VmV(float[] V1, float[] V2)
	{
		float[] Vr = new float[3];
		Vr[0] = V1[0] - V2[0];
		Vr[1] = V1[1] - V2[1];
		Vr[2] = V1[2] - V2[2];
		return Vr;
	}

	public static float[] MTxV(float[][] M1, float[] V1)
	{
		float[] Vr = new float[3];
		Vr[0] = (M1[0][0] * V1[0] +
				M1[1][0] * V1[1] + 
				M1[2][0] * V1[2]); 
		Vr[1] = (M1[0][1] * V1[0] +
				M1[1][1] * V1[1] + 
				M1[2][1] * V1[2]);
		Vr[2] = (M1[0][2] * V1[0] +
				M1[1][2] * V1[1] + 
				M1[2][2] * V1[2]); 
		return Vr;
	}

	public static float[][] MxM(float[][] M1, float[][] M2)
	{
		float[][] Mr = new float[3][3];
		Mr[0][0] = (M1[0][0] * M2[0][0] +
				M1[0][1] * M2[1][0] +
				M1[0][2] * M2[2][0]);
		Mr[1][0] = (M1[1][0] * M2[0][0] +
				M1[1][1] * M2[1][0] +
				M1[1][2] * M2[2][0]);
		Mr[2][0] = (M1[2][0] * M2[0][0] +
				M1[2][1] * M2[1][0] +
				M1[2][2] * M2[2][0]);
		Mr[0][1] = (M1[0][0] * M2[0][1] +
				M1[0][1] * M2[1][1] +
				M1[0][2] * M2[2][1]);
		Mr[1][1] = (M1[1][0] * M2[0][1] +
				M1[1][1] * M2[1][1] +
				M1[1][2] * M2[2][1]);
		Mr[2][1] = (M1[2][0] * M2[0][1] +
				M1[2][1] * M2[1][1] +
				M1[2][2] * M2[2][1]);
		Mr[0][2] = (M1[0][0] * M2[0][2] +
				M1[0][1] * M2[1][2] +
				M1[0][2] * M2[2][2]);
		Mr[1][2] = (M1[1][0] * M2[0][2] +
				M1[1][1] * M2[1][2] +
				M1[1][2] * M2[2][2]);
		Mr[2][2] = (M1[2][0] * M2[0][2] +
				M1[2][1] * M2[1][2] +
				M1[2][2] * M2[2][2]);
		return Mr;
	}

	public static float[] sMTxV(float s1, float[][] M1, float[] V1)
	{
		float[] Vr = new float[3];
		Vr[0] = s1*(M1[0][0] * V1[0] +
				M1[1][0] * V1[1] + 
				M1[2][0] * V1[2]); 
		Vr[1] = s1*(M1[0][1] * V1[0] +
				M1[1][1] * V1[1] + 
				M1[2][1] * V1[2]);
		Vr[2] = s1*(M1[0][2] * V1[0] +
				M1[1][2] * V1[1] + 
				M1[2][2] * V1[2]); 
		return Vr;
	}

	public static float Vlength(float[] V)
	{
		return sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
	}

	public static float[][] McM(Matrix3f M)
	{
		float[][] Mr = new float[3][3];
		Mr[0][0] = M.m00;  Mr[0][1] = M.m01;  Mr[0][2] = M.m02;
		Mr[1][0] = M.m10;  Mr[1][1] = M.m11;  Mr[1][2] = M.m12;
		Mr[2][0] = M.m20;  Mr[2][1] = M.m21;  Mr[2][2] = M.m22;
		return Mr;
	}

	public static float[][] McM(float[][] M)
	{
		float[][] Mr = new float[3][3];
		Mr[0][0] = M[0][0];  Mr[0][1] = M[0][1];  Mr[0][2] = M[0][2];
		Mr[1][0] = M[1][0];  Mr[1][1] = M[1][1];  Mr[1][2] = M[1][2];
		Mr[2][0] = M[2][0];  Mr[2][1] = M[2][1];  Mr[2][2] = M[2][2];
		return Mr;
	}

	public static float[][] Midentity()
	{
		float[][] M = new float[3][3];
		M[0][0] = M[1][1] = M[2][2] = 1;
		M[0][1] = M[1][2] = M[2][0] = 0;
		M[0][2] = M[1][0] = M[2][1] = 0;
		return M;
	}


	public static Matrix3f ROT(Matrix3f a, int i,int j,int k,int l, float tau, float s) {
		float g,h;
		g=a .getElement(i,j);
		h=a.getElement(k,l);
		a.setElement(i,j,(g-s*(h+g*tau)));
		a.setElement(k,l,(h+s*(g-h*tau)));
		return a;
	}

	public static float[] VcV(float[] V)
	{
		float[] Vr = new float[3];
		Vr[0] = V[0];  Vr[1] = V[1];  Vr[2] = V[2];
		return Vr;
	}

	public static float myfabs(float x) {
		return ((x < 0) ? -x : x);
	}

	public static float rfabs(float x) {
		return ((x < 0) ? -x : x);
	}

	public static float fabs(float d) {
		return Math.abs(d);
	}

	public static float sqrt(float d12) {
		return (float) Math.sqrt(d12);
	}

	public static ArrayList minmax(float[] mn,float[] mx,float[] v)
	{
		ArrayList vett = new ArrayList();
		for(int i=0; i<mn.length; i++) {
			if (v[i] < mn[i]) mn[i] = v[i];
			else if (v[i] > mx[i]) mx[i] = v[i];
		}
		vett.add(mn);
		vett.add(mx);
		return vett;
	}


	/**
	 * Il metodo estrae da un oggetto HTransform
	 * la matrice di rotazione
	 * @param ht
	 * @return
	 */
	public static float[][] HTransform2R(HTransform ht) {
		float[][] m = new float[3][3];
		m[0][0] = ht.matrix.m00; m[0][1] = ht.matrix.m01; m[0][2] = ht.matrix.m02;
		m[1][0] = ht.matrix.m10; m[1][1] = ht.matrix.m11; m[1][2] = ht.matrix.m12;
		m[2][0] = ht.matrix.m20; m[2][1] = ht.matrix.m21; m[2][2] = ht.matrix.m22;
		return m;
	}

	/**
	 * Il metodo crea un oggetto HTransform 
	 * da una matrice di rotazione
	 * @param R
	 * @return
	 */
	public static HTransform R2HTransform(float[][] R) {
		HTransform ht = new HTransform();
		for(int i=0; i<3; i++) for(int j=0; j<3; j++) ht.matrix.setElement(i, j, (float) R[i][j]);
		return ht;
	}

	/**
	 * Il metodo crea un oggetto HTransform
	 * da un vettore di traslazione
	 * @param T
	 * @return
	 */
	public static HTransform T2HTransform(float[] T) {
		HTransform ht = new HTransform();
		for(int i=0; i<3; i++) ht.matrix.setElement(i, 3, (float) T[i]);
		return ht;
	}

	/**
	 * Il metodo crea un oggetto HTransform
	 * da un vettore di traslazione e da una 
	 * matrice di rotazione
	 * @param R
	 * @param T
	 * @return
	 */
	public static HTransform R_T2HTransform(float[][] R, float[] T) {
		HTransform ht = new HTransform();
		for(int i=0; i<3; i++) for(int j=0; j<3; j++) ht.matrix.setElement(i, j, (float) R[i][j]);
		for(int i=0; i<3; i++) ht.matrix.setElement(i, 3, (float) T[i]);
		return ht;
	}

	public static float[][] Matrix3f2MFloat(Matrix3f matrix) {
		float[][] m = new float[3][3];
		m[0][0] = matrix.m00; m[0][1] = matrix.m01; m[0][2] = matrix.m02;
		m[1][0] = matrix.m10; m[1][1] = matrix.m11; m[1][2] = matrix.m12;
		m[2][0] = matrix.m20; m[2][1] = matrix.m21; m[2][2] = matrix.m22;
		return m;
	}

	public static Matrix3f MFloat2Matrix3f2(float[][] m) {
		Matrix3f matrix = new Matrix3f();
		for(int i=0; i<3; i++) for(int j=0; j<3; j++) matrix.setElement(i, j, m[i][j]);
		return matrix;
	}

	/**
	 * Estra il vettore di traslazione dall'oggetto
	 * HTransform
	 * @param ht
	 * @return
	 */
	public static float[] HTransform2T(HTransform ht) {
		float[] t = {0,0,0};
		t[0] = ht.matrix.m03; 
		t[1] = ht.matrix.m13; 
		t[2] = ht.matrix.m23;
		return t;
	}

	public static float[] FloatXYZ2Vett(FloatXYZ pos) {
		float[] vett = {0,0,0};
		vett[0] = pos.x;
		vett[1] = pos.y;
		vett[2] = pos.z;
		return vett;
	}



}
